function [x_e,y_e,z_e] = P_estimate(T,S1,S2,S3,sigma_s,sigma_theta,sigma_fi)
% in   T:目标真实坐标 3个测向站 1个目标
% out   目标估计坐标

% 坐标
x = T(1); y = T(2); z = T(3);
x1 = S1(1); y1 = S1(2); z1 = S1(3);
x2 = S2(1); y2 = S2(2); z2 = S2(3);
x3 = S3(1); y3 = S3(2); z3 = S3(3);

% 监测站1 2 3 坐标（带误差）
x1_r = normrnd(x1,sigma_s); y1_r = normrnd(y1,sigma_s); z1_r = normrnd(z1,sigma_s);
x2_r = normrnd(x2,sigma_s); y2_r = normrnd(y2,sigma_s); z2_r = normrnd(z2,sigma_s);
x3_r = normrnd(x3,sigma_s); y3_r = normrnd(y3,sigma_s); z3_r = normrnd(z3,sigma_s);

% 根据目标和测向站的真实坐标 计算真实角度（不带误差）
theta1 = atan((x-x1)/(y-y1));
r1 = sqrt((x-x1)^2+(y-y1)^2);
fi1 = atan((z-z1)/r1);
theta2 = atan((x-x2)/(y-y2));
r2 = sqrt((x-x2)^2+(y-y2)^2);
fi2 = atan((z-z2)/r2);
theta3 = atan((x-x3)/(y-y3));
r3 = sqrt((x-x3)^2+(y-y3)^2);
fi3 = atan((z-z3)/r3);

% 测量站测得的方位角（带误差）
theta1_r = normrnd(theta1,sigma_theta);
theta2_r = normrnd(theta2,sigma_theta);
theta3_r = normrnd(theta3,sigma_theta);

% 计算目标估计位置 x_e,y_e
C = [1 -tan(theta1_r);
     1 -tan(theta2_r);
     1 -tan(theta3_r);
     ];
B = [x1_r-y1_r*tan(theta1_r);
     x2_r-y2_r*tan(theta2_r);
     x3_r-y3_r*tan(theta3_r);
     ];
X = C\B;
x_e = X(1);y_e = X(2);

% 计算目标估计高度 z_e
r1_e = sqrt((x_e-x1_r)^2+(y_e-y1_r)^2);
r2_e = sqrt((x_e-x2_r)^2+(y_e-y2_r)^2);
r3_e = sqrt((x_e-x3_r)^2+(y_e-y3_r)^2);
fi1_r = normrnd(fi1,sigma_fi);
fi2_r = normrnd(fi2,sigma_fi);
fi3_r = normrnd(fi3,sigma_fi);
z_e = (r1_e*tan(fi1_r)+z1_r+r2_e*tan(fi2_r)+z2_r+r3_e*tan(fi3_r)+z3_r)/3;
end

